#!/usr/bin/env python3
import math
import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import Twist, Pose2D, PoseStamped
from tf_transformations import euler_from_quaternion

class Controller:
    def __init__(self, kp, kd):
        self.kp = kp            # P比例系数
        self.kd = kd            # D项
        self.prev_error = 0.0   # 上一次的误差
        self.error_history = [] # 存储上一次误差
        self.current_linear_vel = 0.0
        self.current_angular_vel = 0.0
        self.control_timer = self.create_timer(0.1, self.control_cycle)

    def update(self, error, dt):        # PD控制器更新函数
        # 比例项
        p = self.kp * error         # p环输出等于P项值乘以误差
        # 微分项
        d = self.kd * (error - self.prev_error) / dt if dt > 0 else 0   # 微分项输出等于D项值乘以(当前误差-上一次误差)/dt
        self.prev_error = error     # 更新上一次误差
        self.error_history.append(error)  # 把误差值前值添加到列表中
        return p + d   # 返回PD控制器输出

class Turtlebot(Node):      # Turtlebot类
    def __init__(self):
        super().__init__("turtlebot_move")      # 创建节点
        self.get_logger().info("Press Ctrl + C to terminate")            # 输出提示信息
        self.vel = Twist()                      # 创建速度消息
        self.vel_pub = self.create_publisher(Twist, "cmd_vel", 10)      # 创建发布者，发布cmd_vel话题
        self.pid_theta = Controller(1.0, 0.2)  # PD控制器参数，用于角速度控制
        self.pid_linear = Controller(0.5, 0.1)  # PD控制器参数，用于线速度控制

        # Reset odometry
        self.reset_pub = self.create_publisher(Empty, "reset_odometry", 10)     # 创建发布者，发布reset_odometry话题，该话题用于旧方法重置
        self.get_logger().info("Resetting odometry...")
        self.timer = self.create_timer(0.1, self.publish_reset)                 # 创建定时器，定时重置
        self.reset_count = 10                                                   # 重置次数

        # Subscribe to odometry
        self.pose = Pose2D()        # 创建Pose2D消息
        self.logging_counter = 0    # 记录日志次数
        self.trajectory = []        # 创建轨迹列表
        self.odom_sub = self.create_subscription(Odometry, "odom", self.odom_callback, 10)  # 创建订阅者，订阅odom话题 odom话题用于获取当前位置和角度  
        # 添加轨迹可视化发布器
        self.path_publisher = self.create_publisher(Path, '/trajectory_path', 10)
        self.path_msg = Path()
        self.path_msg.header.frame_id = "odom"  # 设置坐标系

        try:
            self.run()
        except KeyboardInterrupt:
            self.get_logger().info("Action terminated.")
        finally:
            np.savetxt('trajectory.csv', np.array(self.trajectory), fmt='%f', delimiter=',')
            self.get_logger().info("Trajectory saved.")

    def publish_reset(self):
        if self.reset_count > 0:
            self.reset_pub.publish(Empty())
            self.reset_count -= 1
        else:
            self.timer.cancel()
            self.get_logger().info("Odometry reset.")
        

    def turn_to_angle(self, target_angle, tolerance=0.01):  # 旋转到目标角度
        current_angle = self.pose.theta                     # 获取当前角度
        error = target_angle - current_angle                # 计算角度误差
        while abs(error) > tolerance:
            # 使用PD控制器计算角速度
            angular_vel = self.pid_theta.update(error, 0.1) #根据角度误差计算角速度
            self.vel.angular.z = angular_vel                # 设置角速度
            self.vel_pub.publish(self.vel)                  # 发布速度指令
            error = target_angle - self.pose.theta
            self.get_logger().info(f"转向目标角度: {target_angle:.2f}, 当前角度: {self.pose.theta:.2f}, 误差: {error:.2f}")
            self.timer = self.create_timer(0.1, lambda: None)
            self.timer.cancel()
        self.stop()

    def drive_straight(self, target_distance, tolerance=0.01):
        start_x = self.pose.x   # 获取起始位置
        start_y = self.pose.y   
        distance = 0.0          # 初始化距离
        while abs(distance - target_distance) > tolerance:  # 判断是否达到目标距离
            # 使用PD控制器计算线速度
            linear_vel = self.pid_linear.update(target_distance - distance, 0.1)    # 根据目标距离和当前距离计算线速度
            self.vel.linear.x = linear_vel  # 设置线速度
            self.vel_pub.publish(self.vel)  # 发布速度指令
            distance = math.sqrt((self.pose.x - start_x)**2 + (self.pose.y - start_y)**2)   # 计算当前距离
            self.get_logger().info(f"行驶目标距离: {target_distance:.2f}, 当前距离: {distance:.2f}, 误差: {target_distance - distance:.2f}")
            self.timer = self.create_timer(0.1, lambda: None)
            self.timer.cancel()
        self.stop()

    def move_to_point(self, x, y, tolerance=0.1):
        target_x = x
        target_y = y
        while True:
            # 计算到目标点的向量
            dx = target_x - self.pose.x
            dy = target_y - self.pose.y
            distance = math.sqrt(dx**2 + dy**2)
            
            if distance < tolerance:
                break
            
            # 计算目标角度
            target_angle = math.atan2(dy, dx)
            self.turn_to_angle(target_angle, tolerance=0.01)
            
            # 驾驶到目标点
            self.drive_straight(distance, tolerance=0.01)
            
            # 检查是否到达目标点
            dx = target_x - self.pose.x
            dy = target_y - self.pose.y
            distance = math.sqrt(dx**2 + dy**2)
            if distance < tolerance:
                break
        self.stop()

    def stop(self):
        """Stop the robot."""
        self.vel.linear.x = 0.0
        self.vel.angular.z = 0.0
        self.vel_pub.publish(self.vel)
        self.get_logger().info("Robot stopped.")

    def odom_callback(self, msg):
        quaternion = [
            msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.y,
            msg.pose.pose.orientation.z,
            msg.pose.pose.orientation.w,
        ]
        _, _, yaw = euler_from_quaternion(quaternion)
        self.pose.theta = yaw
        self.pose.x = msg.pose.pose.position.x
        self.pose.y = msg.pose.pose.position.y
        #self.vel.linear.x = msg.twist.twist.linear.x
        #self.vel.angular.z = msg.twist.twist.angular.z
        self.current_linear_vel = msg.twist.twist.linear.x
        self.current_angular_vel = msg.twist.twist.angular.z

        self.logging_counter += 1
        if self.logging_counter == 100:
            self.logging_counter = 0
            self.trajectory.append([self.pose.x, self.pose.y])
            self.get_logger().info(f"odom: x={self.pose.x:.2f}; y={self.pose.y:.2f}; theta={yaw:.2f}")
        # 添加轨迹记录
        if self.logging_counter % 10 == 0:  # 降低发布频率
            pose_stamped = PoseStamped()
            pose_stamped.header.stamp = self.get_clock().now().to_msg()
            pose_stamped.header.frame_id = "odom"
            pose_stamped.pose = msg.pose.pose
            self.path_msg.poses.append(pose_stamped)
            self.path_publisher.publish(self.path_msg)
    def control_cycle(self):
        # 将原run方法的逻辑移动到这里
        # 实现周期性的控制指令计算
        # 定义控制序列
        motion_sequence = [
            (0.5, 0.0, 2.0),  # 前进0.5m/s，持续2秒
            (0.0, 0.5, 3.0),  # 原地左转0.5rad/s，持续3秒
            (0.5, 0.0, 2.0),  # 前进0.5m/s，持续2秒
            (0.0, -0.5, 3.0)  # 原地右转-0.5rad/s，持续3秒
        ]
        
        current_step = 0
        start_time = self.get_clock().now()                                  # 获取起始时间
        
        while current_step < len(motion_sequence):
            current_time = self.get_clock().now()                            # 获取当前时间
            elapsed_time = (current_time - start_time).nanoseconds / 1e9     # 计算已执行的时间
            
            target_linear_vel, target_angular_vel, duration = motion_sequence[current_step]    # 目标线速度控制 角度控制 持续时间 从控制序列中获取
            
            if elapsed_time >= duration:         # 如果已执行持续时间，则进入下一步骤
                current_step += 1                # 进入下一步
                start_time = current_time        # 重置起始时间
                if current_step < len(motion_sequence):  # 如果还有步骤，则输出当前步骤
                    self.get_logger().info(f"执行步骤 {current_step + 1}/{len(motion_sequence)}")
                continue
            
            # 使用PD控制器计算速度
            #linear_vel = self.pid_linear.update(target_linear_vel - self.vel.linear.x, 0.1)     # 计算线速度 用目标线速度减去当前线速度，得到误差，
            #使用PD控制器计算线速度
            #angular_vel = self.pid_theta.update(target_angular_vel - self.vel.angular.z, 0.1)     # 计算角速度 用目标角速度减去当前角速度，得到误差
            linear_vel = self.pid_linear.update(target_linear_vel - self.current_linear_vel, 0.1)
            angular_vel = self.pid_theta.update(target_angular_vel - self.current_angular_vel, 0.1)
            
            # 限制速度范围
            linear_vel = max(min(linear_vel, 1.0), -1.0)
            angular_vel = max(min(angular_vel, 2.0), -2.0)
            
            # 发布速度指令
            self.vel.linear.x = linear_vel
            self.vel.angular.z = angular_vel
            self.vel_pub.publish(self.vel)
            
            self.get_logger().info(f"当前执行步骤 {current_step + 1}: "
                                  f"目标线速度={target_linear_vel:.2f} m/s, 实际线速度={self.vel.linear.x:.2f} m/s, "
                                  f"目标角速度={target_angular_vel:.2f} rad/s, 实际角速度={self.vel.angular.z:.2f} rad/s, "
                                  f"已执行 {elapsed_time:.1f}/{duration} 秒")
            
            # 等待下一个控制周期
            self.timer = self.create_timer(0.1, lambda: None)
            self.timer.cancel()


def main(args=None):
    rclpy.init(args=args)
    turtlebot = Turtlebot()
    try:
        rclpy.spin(turtlebot)
    except KeyboardInterrupt:
        turtlebot.get_logger().info("Action terminated.")
    finally:
        turtlebot.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()